#include "cwind.h"

uint8_t CWindBuf[100];
BOOLDef BufStatus = false;
int receiveA;
BfloatDef receiveB;
// 数值解析函数，传入初始位置和结束符，获取数值,并且获得分号的位置
int Numerical_analysis(uint8_t *NumStartIndex, char endchar, uint8_t **header)
{
	int value = 0;
	BOOLDef NegFlag = false;
	while (1)
	{
		if (*NumStartIndex == endchar)
			break;
		if (*NumStartIndex == '-')
		{
			NegFlag = true;
			NumStartIndex++;
			continue;
		}
		value *= 10;
		value += (*NumStartIndex - '0');
		NumStartIndex++;
	}
	*header = ++NumStartIndex;
	if (NegFlag == true)
		value = -value;
	return value;
}

// 浮点数值解析函数，传入初始位置和结束符，获取数值,并且获得分号的位置
float FloatNum_analysis(uint8_t *NumStartIndex, char endchar, uint8_t **header)
{
	float value = 0;
	float value1 = 0;
	int FraFlag = 0;		 // 小数部分标志
	BOOLDef NegFlag = false; // 负数标志
	while (1)
	{
		if (*NumStartIndex == endchar)
			break;
		if (*NumStartIndex == '.')
		{
			FraFlag = 1;
			NumStartIndex++;
		}
		if (*NumStartIndex == '-')
		{
			NegFlag = true;
			NumStartIndex++;
			continue;
		}
		if (FraFlag == 0)
		{
			value *= 10;
			value += (*NumStartIndex - '0');
		}
		else
		{
			value1 *= 10;
			value1 += (*NumStartIndex - '0');
			FraFlag++;
		}
		NumStartIndex++;
	}
	*header = ++NumStartIndex;
	// 是否有小数部分，若有更新
	if (FraFlag != 0)
		value = (value + value1 / pow(10, FraFlag - 1));
	// 是否为负数，若是则更新
	if (NegFlag == true)
		value = -value;
	return value;
}

// 读取速度
BfloatDef cw_getspeed(void)
{
	CWINDSEND((uint8_t *)"DAQBVR;", 7);
	BufStatus = false;
	while (1)
	{
		if (BufStatus == true)
			return receiveB;
	}
}

// 读取路程
BfloatDef cw_getpostition(void)
{
	CWINDSEND((uint8_t *)"DAQBPR;", 7);
	BufStatus = false;
	while (1)
	{
		if (BufStatus == true)
			return receiveB;
	}
}

// 读取剩余路程
BfloatDef cw_getrestpostition(void)
{
	CWINDSEND((uint8_t *)"DAQBPD;", 7);
	BufStatus = false;
	while (1)
	{
		if (BufStatus == true)
			return receiveB;
	}
}

// 读取控制标志
CtrlFlagDef cw_getcontrolflag(void)
{
	CWINDSEND((uint8_t *)"DAQBCF;", 7);
	BufStatus = false;
	while (1)
	{
		if (BufStatus == true)
			return (CtrlFlagDef)receiveA;
	}
}

// 路程阻塞函数
int waitposfor(double diearea)
{
	BfloatDef temp;
	while (1)
	{
		temp = cw_getrestpostition();
		if ((floatabs(temp.leftval) < diearea) && floatabs(temp.rightval) < diearea)
		{
			return 1;
		}
	}
}
// 设置控制标志
void cw_setcontrolflag(CtrlFlagDef flag)
{
	char txt[10];
	sprintf(txt, "DASBCF%d;", (int)flag);
	CWINDSEND((uint8_t *)txt, sizeof(txt) - 1);
}

// 清空路程
void cw_clearpostition(void)
{
	CWINDSEND((uint8_t *)"DASBPC;", 7);
}

// 刹车
void cw_brake(void)
{
	CWINDSEND((uint8_t *)"DASBVT0;", 8);
}

// 模型控制
void cw_modelctrl(float velocity, float omega, float distance)
{
	char txt[50];
	sprintf(txt, "DFMV%fO%fA%f;", velocity, omega, distance);
	CWINDSEND((uint8_t *)txt, sizeof(txt) - 1);
}

// 速度控制
void cw_spinfor(float leftspeed, float rightspeed)
{
	char txt[50];
	sprintf(txt, "SPFl%fr%f;", leftspeed, rightspeed);
	CWINDSEND((uint8_t *)txt, sizeof(txt) - 1);
}

// 速度路程控制
void cw_spinto(float leftspeed, float rightspeed, float leftdistance, float rightdistance)
{
	char txt[50];
	sprintf(txt, "SPTl%fr%fL%fR%f;", leftspeed, rightspeed, leftdistance, rightdistance);
	CWINDSEND((uint8_t *)txt, sizeof(txt) - 1);
}

void cwindUartISR(void)
{
	if ((CWindBuf[0] == 'R' && CWindBuf[1] == 'V') || (CWindBuf[0] == 'R' && CWindBuf[1] == 'P'))
	{
		uint8_t *tempheader = &CWindBuf[3];
		if (*tempheader == 'L')
		{
			receiveB.leftval = FloatNum_analysis(tempheader + sizeof(uint8_t), ',', &tempheader);
		}
		if (*tempheader == 'R')
		{
			receiveB.rightval = FloatNum_analysis(tempheader + sizeof(uint8_t), ';', &tempheader);
		}
		BufStatus = true;
	}
	if (CWindBuf[0] == 'R' && CWindBuf[1] == 'C' && CWindBuf[2] == 'F')
	{
		uint8_t *tempheader = &CWindBuf[2];
		receiveA = Numerical_analysis(tempheader + sizeof(uint8_t), ';', &tempheader);
	}

	BufStatus = true;
}

#ifdef MCU_STM32
void Uart_RxAgain(UART_HandleTypeDef *huart)
{
	HAL_UARTEx_ReceiveToIdle_IT(huart, CWindBuf, 100);
}
#endif